#include "blob_detect.h"
#include "highgui.h"
#include "cv.h"
#include <iostream>
#include <stdio.h>
#include <conio.h>
#include <cxcore.h>

// Function to load the input image and converd it to an binary image.
void BlobDetector::loadImage(const string& filename){
	Mat InputImage =imread(filename);
	cvtColor(InputImage, GrayscaleImage, CV_BGR2GRAY);
	BinaryImage = GrayscaleImage > 128;
	BinaryImage.copyTo(Image);
};

void BlobDetector::LoadLiveFeed(VideoCapture Cam){

	// Initialize capturing live feed from the camera
	Cam >> Frame;
	Frame.copyTo(Image);

	//Convert BRG input image to HSV for thresholding
	cvtColor(Frame, ImgHSV, CV_BGR2HSV, 0);

	// Values 20,100,100 to 30,255,255 working perfect for yellow at around 6pm
	//inRange(ImgHSV, Scalar(24, 200, 200), Scalar(32, 255, 255), WorkImage); // yellow
	//inRange(ImgHSV, Scalar(0, 175, 200), Scalar(3, 255, 255), WorkImage); // Red
	inRange(ImgHSV, Scalar(85, 100, 100), Scalar(130, 255, 255), WorkImage); // Blue
	//inRange(ImgHSV, Scalar(35, 100, 100), Scalar(80, 255, 255), WorkImage); // Green
	//inRange(ImgHSV, Scalar(0, 0, 0), Scalar(0, 0, 4), WorkImage); // Black/virgil
	BackgroundBorder();
};

void BlobDetector::Crosshair(){
	int x, y = 0;
	Point XL, XR, YL, YR;
	namedWindow("Result");

	x = Image.cols / 2;
	y = Image.rows / 2;

	XL.x = x-3;
	XL.y = y;
	XR.x = x+3;
	XR.y = y;

	YL.x = x;
	YL.y = y-3;
	YR.x = x;
	YR.y = y+3;

	VideoCapture cap(0);
	cap >> Frame;
	Frame.copyTo(Image);
	if((x != 0) && (y !=0)){
		line(Image, XL, XR, Scalar(0,0,1), 1, 8, 0);
		line(Image, YL, YR, Scalar(0,0,1), 1, 8, 0);
		imshow("Result", Image);
	};
};

void BlobDetector::blank()
{
	VideoCapture cap(1);
	cap >> Frame;
};

// Function to create a background color border the size of one pixel in width.
//Mat BackgroundBorder(Mat Image){
void BlobDetector::BackgroundBorder(){
	for(int row = 0; row < WorkImage.rows; row++){
		for(int col = 0; col < WorkImage.cols; col++){
			if((row == 0) || (col == 0) || (col == WorkImage.cols - 1) || (row == WorkImage.rows - 1)){
				WorkImage.at<uchar>(row,col) = 0;
			};
		};
	};
};

// movement functions
// & give the adress of the variable, this way we can put the modified value in the aderess of the variable
//*  get the value of where the pointer varialbe points to. The pointer variable contains the memory adress of the variable.
void BlobDetector::MoveUp(){
	PrevMove = PREVUP;
	y--;
	Row--;
	if(y < MinRow){
		MinRow = y;
	};

	SumCol = SumCol - Col  * Col;
};

void BlobDetector::MoveDown(){
	PrevMove = PREVDOWN;
	y++;
	Row++;
	if(y > MaxRow){
		MaxRow = y;
	};

	SumCol = SumCol + Col  * Col;
};

void BlobDetector::MoveLeft(){
	PrevMove = PREVLEFT;
	x--;
	Col--;
	Area = Area + Row;

	if(x < MinCol){
		MinCol = x;
	};

	SumRow = SumRow + Row * Row;
};

void BlobDetector::MoveRight(){
	PrevMove = PREVRIGHT;
	x++;
	Col++;
	Area = Area - Row;

	if(x > MaxCol){
		MaxCol= x;
	};

	SumRow = SumRow - Row * Row;
};

// main routine
int BlobDetector::Image_Scan(){

	uchar* ImagePtr;
	Scratch = Mat::zeros(WorkImage.rows, WorkImage.cols,CV_8UC1);
	bool Done = FALSE;
	int PixelMask = 0;
	Ycor=y;
	Xcor=x;
	AreaL = 0;
	MassRowL= 0;
	MassColL = 0;
	SumRow = 0;
	SumCol = 0;
	Row = 0;
	Col = 0;

	//Scan image horizontally
	//Check first pixel colour
	if (WorkImage.ptr<uchar>(0)[0] == 0){
		PrevPixel = BLACK;
	}
	else{
		PrevPixel = WHITE;
	};

	for(int Row=0; Row < WorkImage.rows; Row++){
		ImagePtr= WorkImage.ptr<uchar>(Row);
		for(int Col=0; Col < WorkImage.cols; Col++){
			if(((ImagePtr[Col] == 255) && ( PrevPixel == 0)) && (Scratch.at<uchar>(Row, Col) == 0)){
				y = Row;
				x = Col;
				Follow_Contour();
				PrevPixel = ImagePtr[Col];
			}
			else{
				PrevPixel = WorkImage.ptr<uchar>(Row)[Col];
			};
		};
	};

	//Check if the a new centre of mass position is changed great enought, if not then the last valid position will be unchanged.
	if((MassColL > LastMassCol +10) || (MassColL < LastMassCol -10) || (MassRowL > LastMassRow +10) || (MassRowL < LastMassRow -10)){
		LastMassCol = MassColL;
		LastMassRow = MassRowL;
	};

	if((LastMassCol > 0) && (LastMassRow > 0) && (LastMassCol < Image.cols) && (LastMassRow < Image.rows)){
		line(Image, Point(LastMassCol, 0), Point(LastMassCol, Image.rows), Scalar(0,255,0), 1, 8, 0);
		line(Image, Point(0, LastMassRow), Point(Image.cols, LastMassRow), Scalar(0,255,0), 1, 8, 0);
		Image.at<Vec3b>(LastMassRow,LastMassCol)[1] = 255;
		Image.at<Vec3b>(Frame.rows / 2, Frame.cols / 2)[1] = 255;
		Image.at<Vec3b>(Frame.rows / 2, (Frame.cols / 2) + 1)[1] = 255;
		Image.at<Vec3b>(Frame.rows / 2,  (Frame.cols / 2) - 1)[1] = 255;
		Image.at<Vec3b>((Frame.rows / 2) + 1, Frame.cols / 2)[1] = 255;
		Image.at<Vec3b>((Frame.rows / 2) - 1, Frame.cols / 2)[1] = 255;
	}


	//Scratch.at<uchar>(LastMassRow,LastMassCol) = 180;
	//namedWindow("Scratch");
	//imshow("Scratch", Scratch);
	namedWindow("Result");
	imshow("Result", Image);

	//Return coordinates, only there is a diffrence else return char[] 0. Build in check to determine return value.

	return 0;
};

void BlobDetector::Follow_Contour(){

	bool Done = FALSE;
	int PixelMask = 0;
	int ContourLenght = 0;
	Ycor=y;
	Xcor=x;
	MinRow = y;
	MinCol = x;
	MaxRow = y;
	MaxCol = x;
	MassRow = 0;
	MassCol = 0;
	Area = 0;

	while(!Done){

		PixelMask = 0;

		// map pixelmask
		if(WorkImage.ptr<uchar>(y)[x] == 255){
			PixelMask = PixelMask + LOWERRIGHT;
		};
		if(WorkImage.ptr<uchar>(y)[x-1] == 255){
			PixelMask =PixelMask + LOWERLEFT;
		};
		if(WorkImage.ptr<uchar>(y-1)[x] == 255){
			PixelMask = PixelMask + UPPERRIGHT;
		};
		if(WorkImage.ptr<uchar>(y-1)[x-1] == 255){
			PixelMask = PixelMask + UPPERLEFT;
		};

		// move in direction
		switch(PixelMask){
		case 1:
			MoveRight();
			break;

		case 2:
			MoveDown();
			break;

		case 3:
			MoveRight();
			break;

		case 4:
			MoveUp();
			break;

		case 5:
			MoveUp();
			break;

		case 6:
			if(PrevMove == 1){
				MoveUp();
				break;
			}
			else{
				MoveDown();
				break;
			};

		case 7:
			MoveUp();
			break;

		case 8:
			MoveLeft();
			break;

		case 9:
			if(PrevMove == 4){
				MoveLeft();
				break;
			}
			else{
				MoveRight();
				break;
			}

		case 10:
			MoveDown();
			break;

		case 11:
			MoveRight();
			break;

		case 12:
			MoveLeft();
			break;

		case 13:
			MoveLeft();
			break;

		case 14:
			MoveDown();
			break;


		default:
			cout << "Error case value: " << int(PixelMask) << endl;
			break;
		}

		Scratch.at<uchar>(y,x) = 255;

		ContourLenght++;

		if((y == Ycor) && (x ==Xcor)){

			//Caluculate the area and find the centre of mass
			//for(int Row = MinRow; Row < MaxRow+1; Row++){
			//		for(int Col = MinCol; Col < MaxCol+1; Col++){

			//			if(WorkImage.at<uchar>(Row,Col) == 255){
			//				Area++;
			//				MassRow = MassRow + Row;
			//				MassCol = MassCol + Col;
			//			};
			//		};
			//};

			//cout << "Area: " << Area << " AreaL: " << AreaL << endl;
			//cout << "Area: " << Area << " AreaL: " << AreaL << endl;
			//Check if the last found blob is bigger then the previous one.
			if((Area > AreaL) && (Area != 0)){
				AreaL = Area;
				MassRowL = y + SumRow / (2 * AreaL);
				MassColL = x + SumCol / (2 * AreaL);
			};
			Done = TRUE;
		};
	};
};
